#include "pid.h"
#include "encoder.h"
#include "ble.h"
#include "motor.h"
#include <math.h>
#include "filter.h"

//#include "string.h"

pid_pos pid_angle =   {.Kp = 0, .Ki = 0, .Kd = 0,.integral_max=0,
.output_max = 34,.target=0,.now=0,.last_now=0,.integral=0,.output=0,.last_error=0};//output_max要根据实测速度重新设置,不加低通174,加了就34

pid_inc pid_speed =   {.Kp = 0.6, .Ki = 0.02, .Kd = 0,
	.output_max = 100,.target=0,.now=0,.output=0,.last_error=0};												
	
float filtered_speed=0;	
int32_t global_encorder=0;

float pid_cal_pos(pid_pos *pid)
{
    float error = pid->target - pid->now; 
	
#if 0
	// 带死区的积分分离
	if(fabs(error) < 10.0)
	{
			if(fabs(error) >= 0.2) 
			{
					pid->integral += error;
			}
			else
				pid->integral = 0;
	}
	else
	{
			pid->integral = 0;
	}
#endif

    //积分限幅
		if (pid->integral > pid->integral_max) {
				pid->integral = pid->integral_max;
		} else if (pid->integral < -pid->integral_max) {
				pid->integral = -pid->integral_max;
		} 

    float derivative =  -(pid->now - pid->last_now);
		pid->last_now=pid->now;

		//计算输出
		pid->output = pid->Kp * error + pid->Ki * pid->integral + pid->Kd * derivative;

		//输出限幅
    if (pid->output > pid->output_max) pid->output = pid->output_max;
    if (pid->output < -pid->output_max) pid->output = -pid->output_max;
	
		
		if (fabs(error)<0.15) pid->output = 0;
		
    return pid->output;
}

float pid_cal_inc(pid_inc *pid)
{
    // 1. 计算当前误差
    float error = pid->target - pid->now;

    // 2. 计算增量 ΔOutput
    // ΔOutput = Kp * [e(k) - e(k-1)] + Ki * e(k)
    float delta_output = pid->Kp * (error - pid->last_error) + pid->Ki * error;

    // 3. 更新历史误差，为下一次计算做准备
    pid->last_error = error;

    // 4. 累积增量得到当前理论输出
    // Output(k) = Output(k-1) + ΔOutput
    pid->output += delta_output;

    // 5. 处理输出饱和（抗积分饱和）
    if (pid->output > pid->output_max) {
        pid->output = pid->output_max;
    }
    else if (pid->output < -pid->output_max) {
        pid->output = -pid->output_max;
    }
		
#if 0	
		//死区补偿
		float dead_zone=5.4;
		if (pid->output > -dead_zone&&pid->output <0) 
			pid->output -=dead_zone;
		if (pid->output > 0&&pid->output <dead_zone) 
			pid->output += dead_zone;
#endif	
		if(fabs(pid_angle.now-pid_angle.target)<=0.15){
			pid->output=0;
		}
		
    return pid->output;
}

static int32_t absolute_position = 0; 
static int16_t last_encoder_raw = 0; 

void pid_speed_task()
{
    int16_t current_encoder_raw = (int16_t)__HAL_TIM_GET_COUNTER(EN1_TIM);
    int16_t encoder_delta = current_encoder_raw - last_encoder_raw;
		//如果电机在一个采样周期内转动超过了半圈（32767个计数），这个算法会失效。
    if (encoder_delta > 32767/2) // 用范围的一半作为阈值最稳妥
    {
        encoder_delta -= 65536; // 从 (2^16) 中减去，得到真实的负向增量
    }
    else if (encoder_delta < -32767/2)
    {
        encoder_delta += 65536; // 加上 (2^16)，得到真实的正向增量
    }
    absolute_position += encoder_delta;
    last_encoder_raw = current_encoder_raw;

    // a. 将绝对位置用于外环（位置环）
		pid_angle.now =  (float)absolute_position*360/28000.0f;//500ppr*2倍频*28减速比 
		
		// b. 将增量用于内环（速度环）
#if 1
		// 初始化低通滤波器，设置时间常数为0.2
    LowPassFilter myFilter;
    initializeFilter(&myFilter, 0.2);
		pid_speed.now =	filterValue(&myFilter,encoder_delta);
#else	
		pid_speed.now = counter_num;
#endif
	
		Motor_run(pid_cal_inc(&pid_speed));
		
#if pid_debug_pos_inc	
		vofa_send(4,(float)pid_speed.target,(float)pid_speed.now,(float)global_encorder,(float)pid_speed.output);
#else
		vofa_send(4,(float)pid_angle.target ,(float)pid_angle.now ,(float)(pid_angle.now - pid_angle.target) ,(float)pid_angle.output);
#endif	
}

void pid_angle_task()
{
		pid_speed.target=pid_cal_pos(&pid_angle);
}